//
// Created by Hoyin on 2024/4/23.
//

#ifndef VISUAL_LOCATION_FORWARDINTERSECTION_H
#define VISUAL_LOCATION_FORWARDINTERSECTION_H

#include <Eigen/Core>
#include "Coordinate.h"


class ForwardIntersection {
public:
	Eigen::Vector3d static calc(ContinuousOrient::Result& coResult, ImageCoord& imageCoordL, ImageCoord& imageCoordR) {
		Matrix3d E = Matrix3d::Identity();
		ImageAssistCoord iacL(imageCoordL, E);
		ImageAssistCoord iacR(imageCoordR, coResult.RotateMatrix);
		double Bx = coResult.RStation(0);
		double By = coResult.RStation(1);
		double Bz = coResult.RStation(2);
		double X1 = iacL.X();
		double X2 = iacR.X();
		double Y1 = iacL.Y();
		double Y2 = iacR.Y();
		double Z1 = iacL.Z();
		double Z2 = iacR.Z();
		double N1 = (Bx * Z2 - Bz * X2) / (X1 * Z2 - Z1 * X2);
		double N2 = (Bx * Z1 - Bz * X1) / (X1 * Z2 - Z1 * X2);

		double Mx = N1 * X1;
		double My = 0.5*(N1*Y1 + N2*Y2 + By);
		double Mz = N1 * Z1;

		Mx *= 0.001;
		My *= 0.001;
		Mz *= 0.001;

		Vector3d modelPos(Mx, My, Mz);
		return modelPos;
	}
};


#endif //VISUAL_LOCATION_FORWARDINTERSECTION_H
